/*
 * event_queue.c
 *
 * event queue functions
 *
 * Copyright (C) 2020 Long-Horn
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>

#include <sys/syscall.h>
#include <sys/mman.h>
#include <sys/types.h>

#include "event_queue.h"

//#define DEBUG_PRINT

/** Convenience wrappers for erroring out
  */
static inline void queue_error(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    fprintf(stderr, "queue error: ");
    vfprintf(stderr, fmt, args);
    fprintf(stderr, "\n");
    va_end(args);
    abort();
}

static inline void queue_error_errno(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    fprintf(stderr, "queue error: ");
    vfprintf(stderr, fmt, args);
    fprintf(stderr, " (errno %d)\n", errno);
    va_end(args);
    abort();
}

#ifdef DEBUG_PRINT
static void print_event_payload(void *event, char *call_name)
{
    event_s *pevent = (event_s *)event;
    
    printf("\n%s: event->payload_len=%ld \n", call_name, pevent->payload_len);
    
    int i;
    
    for (i=0; i<pevent->payload_len; i++) {
        if( i >0 && (i%8 == 0)) printf("\n");
        printf("0x%02x ", pevent->payload[i]);
    }
    
    printf("\n");
}
#endif

static void event_queue_init(event_queue_s *q, size_t n)
{
    q->buffer = calloc(n, sizeof(event_s));
    if (q->buffer == NULL) {
        queue_error_errno("Could not allocate virtual memory");
    }

    // Initialize synchronization primitives
    if (pthread_mutex_init(&q->mutex, NULL) != 0) {
        queue_error_errno("Could not initialize mutex");
    }
    if(pthread_cond_init(&q->readable, NULL) != 0) {
        queue_error_errno("Could not initialize condition variable");
    }
    if(pthread_cond_init(&q->writeable, NULL) != 0) {
        queue_error_errno("Could not initialize condition variable");
    }
    
    // Initialize remaining members
    q->size = n * sizeof(event_s);
    q->head = 0;
    q->tail = 0;
}

static int event_queue_is_full(void *q, event_s *event)
{
    event_queue_s *pq = (event_queue_s *)q;
    size_t remain_len;
    int full = 0;
    
    if (pq->tail + sizeof(event_s) >= pq->size) {
        remain_len = pq->tail + sizeof(event_s) - pq->size;
        if (remain_len >= pq->head)
                full = 1;
    }
        
    return full;
}

static int event_queue_enqueue(void *q, event_s *event)
{
    event_queue_s *pq;
    
    pq = (event_queue_s *)q;
    pthread_mutex_lock(&pq->mutex);
    
#ifdef DEBUG_PRINT
    print_event_payload(event, "event_queue_enqueue");
#endif

    // Wait for space to become available
    while (event_queue_is_full(q, event)) {
        pthread_cond_wait(&pq->writeable, &pq->mutex);
    }
    
    // Write event
    memcpy(&pq->buffer[pq->tail], (void *)event, sizeof(event_s));
    
    // Increment write index
    pq->tail  += sizeof(event_s);
    
    if(pq->tail >= pq->size) {
        pq->tail -= pq->size;
    }
    
    pthread_cond_signal(&pq->readable);
    pthread_mutex_unlock(&pq->mutex);
    
    return 0;
}

static size_t event_queue_dequeue(void *q, event_s *event)
{
    event_queue_s *pq;
    
    pq = (event_queue_s *)q;
    pthread_mutex_lock(&pq->mutex);
    
    for (;;) {
    
        // Wait for event to arrive
        while ((pq->tail - pq->head) == 0) {
            pthread_cond_wait(&pq->readable, &pq->mutex);
        }
        
        // Get event
        memcpy((void *)event, (void *)&pq->buffer[pq->head], sizeof(event_s));
        
        break;
    }
    
    pq->head += sizeof(event_s);
    
    if (pq->head >= pq->size) {
        pq->head -= pq->size;
    }
    
    pthread_cond_signal(&pq->writeable);
    pthread_mutex_unlock(&pq->mutex);
    
    return sizeof(event_s);
}

int create_event_queue(event_queue_s **event_q, size_t n)
{
    int res = -1;

    event_queue_s *p_event_queue = NULL;
    p_event_queue = (event_queue_s *)malloc(sizeof(event_queue_s));
    if(NULL == p_event_queue) {
        printf("### %s: malloc event_queue_s failed!\n", __func__);
        return res;
    }

    res = 0;
    p_event_queue->enqueue = event_queue_enqueue;
    p_event_queue->dequeue = event_queue_dequeue;
    
    *event_q = p_event_queue;
    
    event_queue_init(p_event_queue, n);
    
    printf("%s: @Line:%d  Ok\n", __func__, __LINE__);
    
    return res;
}

void destroy_event_queue(event_queue_s *event_q)
{
    if (!event_q) {
        if (pthread_mutex_destroy(&event_q->mutex) != 0) {
            queue_error_errno("Could not destroy mutex");
        }
    
        if (pthread_cond_destroy(&event_q->readable) != 0) {
            queue_error_errno("Could not destroy condition variable");
        }
    
        if (pthread_cond_destroy(&event_q->writeable) != 0) {
            queue_error_errno("Could not destroy condition variable");
        }

        if (!event_q->buffer) free(event_q->buffer);
        
        free(event_q);
    }
}
